#include "agv_msgs_converter.h"

Json::Value agv_msgs_to_json(const agv_msgs::msg::AGVMode &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["mode"] = msg.mode;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::AgvAutoStatus &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["taskID"] = Json::Value::LargestUInt(msg.taskid);
    json_msg["subTaskID"] = msg.subtaskid;
    json_msg["fromNode"] = msg.fromnode;
    json_msg["toNode"] = msg.tonode;
    json_msg["fProgress"] = msg.fprogress;
    json_msg["fProgPercent"] = msg.fprogpercent;
    json_msg["curPst_x"] = msg.curpst_x;
    json_msg["curPst_y"] = msg.curpst_y;
    json_msg["curPst_t"] = msg.curpst_t;
    json_msg["curVel_x"] = msg.curvel_x;
    json_msg["curVel_z"] = msg.curvel_z;
    json_msg["velLevel"] = msg.vellevel;
    json_msg["fDevProg"] = msg.fdevprog;
    json_msg["fDev"] = msg.fdev;
    json_msg["fDevThita"] = msg.fdevthita;
    json_msg["fQuality"] = msg.fquality;
    json_msg["event"] = msg.event;
    json_msg["workState"] = msg.workstate;
    json_msg["wheelMoveDir"] = msg.wheelmovedir;
    json_msg["trajType"] = msg.trajtype;
    json_msg["locateType"] = msg.locatetype;
    json_msg["scenes"] = msg.scenes;
    json_msg["bits"] = msg.bits;
    json_msg["word1"] = msg.word1;
    json_msg["word2"] = msg.word2;
    json_msg["pathWord1"] = msg.pathword1;
    json_msg["pathWord2"] = msg.pathword2;
    json_msg["rackFollowUp"] = msg.rackfollowup;
    json_msg["navigationType"] = msg.navigationtype;
    json_msg["driving"] = (bool)msg.driving;
    json_msg["startDelay"] = (bool)msg.startdelay;
    json_msg["paused"] = (bool)msg.paused;
    json_msg["idle"] = (bool)msg.idle;
    json_msg["orderIdNew"] = msg.orderidnew;
    json_msg["orderUpdateIdNew"] = msg.orderupdateidnew;
    json_msg["nodeIdNew"] = msg.nodeidnew;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::BatInfo &msg)
{
    Json::Value json_msg;
    json_msg["chargeFlag"] = (bool)msg.charge_flag;
    json_msg["chargeFinished"] = (bool)msg.charge_finished;
    json_msg["chargeInterrupted"] = (bool)msg.charge_interrupted;
    json_msg["voltage"] = msg.voltage;
    json_msg["current"] = msg.current;
    json_msg["numeric_bat"] = msg.numeric_bat;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::NavType &msg)
{
    Json::Value floorArray(Json::arrayValue);
    for (const auto &floor : msg.floors)
    {
        floorArray.append(floor);
    }

    Json::Value json_msg;
    json_msg["type"] = msg.type;
    json_msg["currentFloor"] = msg.currentfloor;
    json_msg["floors"] = floorArray;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::AGVEventStatus &msg)
{
    Json::Value evArray(Json::arrayValue);
    for (const auto &ev : msg.events)
    {
        Json::Value json_ev;
        json_ev["type"] = ev.type;
        json_ev["action"] = ev.action;
        json_ev["ackReq"] = ev.ackreq;
        json_ev["eventTime"] = ev.eventtime;
        json_ev["velLimit"] = ev.vellimit;
        json_ev["ecode"] = ev.ecode;
        json_ev["param1"] = ev.param1;
        json_ev["param2"] = ev.param2;
        json_ev["param3"] = ev.param3;
        json_ev["param4"] = ev.param4;
        json_ev["description_cn"] = ev.description_cn;
        json_ev["description"] = ev.description;

        evArray.append(json_ev);
    }

    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["node"] = msg.node;
    json_msg["type"] = msg.type;
    json_msg["action"] = msg.action;
    json_msg["ackReq"] = msg.ackreq;
    json_msg["eventTime"] = msg.eventtime;
    json_msg["velLimit"] = msg.vellimit;
    json_msg["ecode"] = msg.ecode;
    json_msg["description"] = msg.description;
    json_msg["events"] = evArray;

    return json_msg;
}

// Json::Value agv_msgs_to_json(const agv_msgs::msg::tape_sensors &msg)
// {
//     Json::Value sensorArray(Json::arrayValue);
//     for (const auto &ssr : msg.sensors)
//     {
//         Json::Value json_ssr;
//         json_ssr["name"] = ssr.name;
//         json_ssr["deviation"] = ssr.deviation;
//         json_ssr["valid"] = (bool)ssr.valid;

//         sensorArray.append(json_ssr);
//     }

//     Json::Value json_msg;
//     json_msg["header"] = std_msgsToJson(msg.header);
//     json_msg["sensors"] = sensorArray;

//     return json_msg;
// }

// Json::Value agv_msgs_to_json(const agv_msgs::msg::rfid_sensor &msg)
// {
//     Json::Value json_msg;
//     json_msg["header"] = std_msgsToJson(msg.header);
//     json_msg["name"] = msg.name;
//     json_msg["tag_id"] = msg.tag_id;
//     json_msg["valid"] = (bool)msg.valid;

//     return json_msg;
// }

Json::Value agv_msgs_to_json(const agv_msgs::msg::ReadInPutsNew &msg)
{
    Json::Value array(Json::arrayValue);
    for (const auto &in : msg.array)
    {
        Json::Value json_in;
        json_in["key"] = in.key;
        json_in["value"] = in.value;

        array.append(json_in);
    }

    Json::Value json_msg;
    json_msg["array"] = array;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::OdometryLite &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["child_frame_id"] = msg.child_frame_id;
    json_msg["pose"] = geometry_msgsToJson(msg.pose);
    json_msg["twist"] = geometry_msgsToJson(msg.twist);

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::LidarCalibPercent &msg)
{
    Json::Value json_msg;
    json_msg["percent"] = msg.percent;
    json_msg["theta"] = msg.theta;
    json_msg["x"] = msg.x;
    json_msg["y"] = msg.y;
    json_msg["theta2"] = msg.theta2;
    json_msg["x2"] = msg.x2;
    json_msg["y2"] = msg.y2;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::QrCalibPercent &msg)
{
    Json::Value json_msg;
    json_msg["percent"] = msg.percent;
    json_msg["theta"] = msg.theta;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::AgvOdomCalib &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["process"] = msg.process;
    json_msg["distance"] = msg.distance;
    json_msg["theta"] = msg.theta;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::DriversInfo &msg)
{
    Json::Value drivArray(Json::arrayValue);
    for (auto &driv : msg.drivers)
    {
        Json::Value json_driv;
        json_driv["jointName"] = driv.jointname;
        json_driv["statusWord"] = driv.statusword;
        json_driv["ready"] = (bool)driv.ready;
        json_driv["motorState"] = driv.motorstate;

        drivArray.append(json_driv);
    }

    Json::Value json_msg;
    json_msg["drivers"] = drivArray;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::CANopenNodeStatus &msg)
{
    Json::Value nodeArray(Json::arrayValue);
    for (auto &node : msg.node_status)
    {
        Json::Value json_node;
        json_node["busname"] = node.busname;
        json_node["nodeid"] = node.nodeid;
        json_node["type"] = node.type;
        json_node["running_status"] = node.running_status;
        json_node["communication_status"] = node.communication_status;
        json_node["timelost"] = node.timelost;

        nodeArray.append(json_node);
    }

    Json::Value json_msg;
    json_msg["node_status"] = nodeArray;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::PoseWithConfidence &msg)
{
    Json::Value json_msg;
    json_msg["score"] = msg.score;
    json_msg["qr_init"] = (bool)msg.qr_init;
    json_msg["zoneTag"] = msg.zonetag;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["pose"] = geometry_msgsToJson(msg.pose);

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::MapPng &msg)
{
    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["data"] = msg.data;
    json_msg["resolution"] = msg.resolution;
    json_msg["width"] = msg.width;
    json_msg["height"] = msg.height;
    json_msg["origin"] = geometry_msgsToJson(msg.origin);

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::CalibProgress &msg)
{
    Json::Value json_msg;
    json_msg["progress"] = msg.progress;

    return json_msg;
}
Json::Value agv_msgs_to_json(const agv_msgs::msg::AvoidObsta &msg)
{
    Json::Value json_msg;
    json_msg["equipment"] = msg.equipment;
    json_msg["area"] = msg.area;
    json_msg["warn"] = (bool)msg.warn;
    json_msg["slow"] = (bool)msg.slow;
    json_msg["stop"] = (bool)msg.stop;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::AvoidObstaInfo &msg)
{
    // Json::Value avoid_msgArray(Json::arrayValue);
    // for (auto &mem : msg.avoid_msg)
    // {
    //     avoid_msgArray.append(agv_msgs_to_json(mem));
    // }

    Json::Value json_msg;
    // json_msg["time"] = std_msgsToJson(msg.time);
    // json_msg["scenes_name"] = msg.scenes_name;
    // json_msg["avoid_msg"] = avoid_msgArray;

    return json_msg;
}

Json::Value agv_msgs_to_json(const agv_msgs::msg::StatusResponse &msg)
{
    Json::Value json_msg;
    json_msg["code"] = msg.code;
    json_msg["message"] = msg.message;

    return json_msg;
}

// Json::Value agv_msgs_to_json(const reflector_slam_ros::msg::ReflectorListPose &msg)
// {
//     Json::Value refArray(Json::arrayValue);
//     for (auto &ref : msg.g_reflector_pose)
//     {
//         Json::Value json_ref;
//         json_ref["reflector_num"] = ref.reflector_num;
//         json_ref["reflector_point"] = geometry_msgsToJson(ref.reflector_point);

//         refArray.append(json_ref);
//     }

//     Json::Value json_msg;
//     json_msg["g_reflector_pose"] = refArray;

//     return json_msg;
// }

// Json::Value agv_msgs_to_json(const reflector_slam_ros::msg::ReflectorStatus &msg)
// {
//     Json::Value json_msg;
//     json_msg["reflector_slam_type"] = std_msgsToJson(msg.reflector_slam_type);
//     json_msg["localization_state"] = std_msgsToJson(msg.localization_state);
//     json_msg["localization_map_id"] = msg.localization_map_id;
//     json_msg["localization_pose"] = geometry_msgsToJson(msg.localization_pose);

//     return json_msg;
// }

Json::Value agv_msgs_to_json(const slam_ros_msgs::msg::MapdataProgress &msg)
{
    Json::Value json_msg;
    json_msg["current_mapfile_name"] = msg.current_mapfile_name;
    json_msg["processed_seconds"] = msg.processed_seconds;
    json_msg["current_progress"] = msg.current_progress;
    json_msg["error"] = msg.error;
    json_msg["status"] = (bool)msg.status;

    return json_msg;
}

Json::Value agv_msgs_to_json(const slam_ros_msgs::msg::RobotStatus &msg)
{
    Json::Value json_msg;
    json_msg["slam_type"] = std_msgsToJson(msg.slam_type);
    json_msg["localization_state"] = std_msgsToJson(msg.localization_state);
    json_msg["localization_map"] = msg.localization_map;
    json_msg["localization_pose"] = geometry_msgsToJson(msg.localization_pose);
    json_msg["laser_to_base"] = geometry_msgsToJson(msg.laser_to_base);

    return json_msg;
}

Json::Value agv_msgs_to_json(const slam_ros_msgs::msg::LandmarkListPose &msg)
{
    Json::Value poseArray(Json::arrayValue);
    for (auto &mem : msg.g_landmark_pose)
    {
        poseArray.append(geometry_msgsToJson(mem));
    }

    Json::Value json_msg;
    json_msg["header"] = std_msgsToJson(msg.header);
    json_msg["g_landmark_pose"] = poseArray;

    return json_msg;
}

Json::Value agv_msgs_to_json(const std_msgs::msg::ColorRGBA &msg)
{
    return std_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const geometry_msgs::msg::PoseStamped &msg)
{
    return geometry_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const geometry_msgs::msg::Twist &msg)
{
    return geometry_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const geometry_msgs::msg::TwistStamped &msg)
{
    return geometry_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const geometry_msgs::msg::Pose &msg)
{
    return geometry_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const geometry_msgs::msg::Vector3 &msg)
{
    return geometry_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const geometry_msgs::msg::PointStamped &msg)
{
    return geometry_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const sensor_msgs::msg::JointState &msg)
{
    return sensor_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const sensor_msgs::msg::PointCloud &msg)
{
    return sensor_msgsToJson(msg);
}

Json::Value agv_msgs_to_json(const sensor_msgs::msg::LaserScan &msg)
{
    return sensor_msgsToJson(msg);
}